/*************************************************************************
*
*	gpsd.c
*
*
*  Copyright 2008,2009 Steve Stancliff
*
*
*  Part of the TAOSF SSA system.
*
*
*  This module reads gps data from a gpsd server.
*
*
*  version 1.0.5, 5/6/09, SBS
*    - add additional GPS inputs: direction, 
*
*  version 1.0.4, 4/24/09, SBS
*    - documentation
*
*  version 1.0.3, 1/9/09, SBS
*    - switch to using callback instead of polling
*
*  version 1.0.2, 1/9/09, SBS
*    - return actual value in StartThread()
*    - LogPrint --> LogLevelPrint
*    - modify mainloop to peridic execution instead of constant delay
*
*  version 1.0.1, 11/26/08, SBS
*    - cleanup
*
*  version 1.0, 10/22/08, SBS
*    - convert from standalone app to module 
*
*  version 0.1, 10/21/08, SBS
*    - first skeletal version 
*
**************************************************************************
*
*  This program is free software; you can redistribute it and/or modify
*  it under the terms of the GNU General Public License as published by
*  the Free Software Foundation; either version 2 of the License, or
*  (at your option) any later version.
*
*  This program is distributed in the hope that it will be useful, but 
*  WITHOUT ANY WARRANTY; without even the implied warranty of 
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
*  General Public License for more details.
*
*  You should have received a copy of the GNU General Public License 
*  along with this program; if not, write to the Free Software Foundation, 
*  Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 
*
**************************************************************************/


/*************
* Portions from cgpxlogger.c 4685 2008-02-17 02:57:48Z ckuethe $ 
* those portions under copyright as follows:
* Copyright (c) 2005,2006 Chris Kuethe <chris.kuethe@gmail.com>
*
* Permission to use, copy, modify, and distribute this software for any
* purpose with or without fee is hereby granted, provided that the above
* copyright notice and this permission notice appear in all copies.
*
* THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
* WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
* ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
* WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
* ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
* OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
*
**************/

/**
*  @file 
*  @ingroup plat_eng_hardware
*  @brief
*  This module reads gps data from a gpsd server.
*  <hr>
**/

#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <string.h>
#include <unistd.h>
#include <math.h>
#include <gps.h>
#include <pthread.h>

#include <time.h>
#include <sys/timeb.h>
#include "delay.h"

#include "waypoints.h"
//#define NEED_MODENAMES
#include "platform_modes.h"
//#undef NEED_MODENAMES
#include "platform_types.h"
#include "status.h"

#include "config.h"
#include "telemfile.h"
//#include "sendretr.h"

#ifdef DEBUG
#include "logfile.h"
#endif

#include "gpsd.h"


typedef struct gps_data_t GPSDATA;

static GPSDATA gpsdata0,gpsdata1;
static int validdata;


pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;



#define FILESPECLEN 256

#define SLEEPDELAY (0.025)
#define LOOPINTERVAL (0.1)


/********************************************************
*
* gpsd_Callback()
*/
/**
* @brief
* The gpsd daemon calls this function when new data 
*    is available.
*
*
********************************************************/
void gpsd_Callback(
		GPSDATA gpsdata,
		char *buf, 
		size_t len, 
		int level)
{

//	printf("gpsd_Callback(): in\n");
//	fflush(stdout);
	
	pthread_mutex_lock( &mutex );
	gpsdata1 = gpsdata;
	pthread_mutex_unlock( &mutex );
	
//	printf("gpsd_Callback(): out\n");
//	fflush(stdout);
	
	validdata = 1;
	
	return;
}


/********************************************************
*
* gpsd_Init()
*/
/**
* @brief
* Opens connection to gpsd daemon.
*
*
********************************************************/
void gpsd_Init(
			char *host
			,char *port)
{
	static pthread_t handler;

	#ifdef DEBUG
	logfile_LogLevelPrint(LOGLEVEL_INFO,"gpsd_Init() : Initializing gps connection");
	#endif

	gpsdata0 = gps_open(host, port);

	if (!gpsdata0) {
		#ifdef DEBUG
		logfile_LogLevelPrint(LOGLEVEL_INFO,"gpsd_Init() : gps_open() Failed");
		#endif
		printf("gpsd_Init() : gps_open() Failed\n");
		exit(1);
	}

	#ifdef DEBUG
	logfile_LogLevelPrint(LOGLEVEL_INFO,"   Success!");
	#endif

//	gps_set_raw_hook(gpsdata0, gpsd_Callback);
	gps_set_callback(gpsdata0, gpsd_Callback, &handler);

	validdata = 0;
	
	return;
}


/********************************************************
*
* gpsd_mainloop()
*/
/**
* @brief
* Continuous loop (threaded) which pulls data from 
*   gpsd at regular intervals.
*
*
********************************************************/
void * gpsd_mainloop(
			void *ptr)
{	
	struct timeb starttime,endtime;
	TIMESPEC delay;
	double delta;

#ifdef POLLED
	fd_set fds;
	struct timeval tv;
	int data;
	char *pollstr = "SPAMDQTV";

	delay = delay_setdelay(SLEEPDELAY);
#endif
	
	while(1){

		ftime(&starttime);
/*
//		pthread_mutex_lock( &mutex );
		FD_ZERO(&fds);
		FD_SET(gpsdata0->gps_fd, &fds);

		if(gps_query(gpsdata0, pollstr) != 0){
				printf("ack2!\n");
				exit(-2);
		}

		tv.tv_usec = 250000;
		tv.tv_sec = 0;

 		data = select(gpsdata0->gps_fd + 1, &fds, NULL, NULL, &tv);

		if (data < 0) {
			printf("gpsd - no data!\n");
			fprintf(stderr,"%s", strerror(errno));
			return;
//			exit(2);
		}else if (data){
			if(gps_poll(gpsdata0) != 0){
				printf("ack1!\n");
				exit(-2);
			}
		}
//		pthread_mutex_unlock( &mutex );
*/		
		ftime(&endtime);	
		
		delta = delay_deltatime(starttime,endtime);
		
		while(delta < (LOOPINTERVAL-SLEEPDELAY)){
			delay_sleep(&delay);
			ftime(&endtime);	
			delta = delay_deltatime(starttime,endtime);		
		}

//		delay_sleep(&delay);
	}	
	
}


/********************************************************
*
* gpsd_GetStatus()
*/
/**
* @brief
* Retrieve current GPS status.
*
*
********************************************************/
void gpsd_GetStatus(
			STATUS *s)
{
	static struct tm *tmp;
	long int l;

	pthread_mutex_lock( &mutex );
	
	if(validdata == 1){

		if (gpsdata1->status > 0){
		
			s->gpsdata = gpsdata1->fix;
		
			if ((s->gpsdata.mode > 1)){
				if(!isnan(s->gpsdata.time)){

					s->dLatDeg = s->gpsdata.latitude;
					s->dLonDeg = s->gpsdata.longitude;
					
					s->dAltMeters = s->gpsdata.altitude;       
					s->dSpeed_mps = s->gpsdata.speed;
//					s->dDirectionDeg = s->gpsdata.track;

					l = (s->gpsdata.time);
					s->nTime = (int)l;
					tmp = localtime(&(l));		
					status_SetTimeString(s->sTime, sizeof(s->sTime),tmp);

				}
			}
		}
	}
	pthread_mutex_unlock( &mutex );

	return;
}


/********************************************************
*
* gpsd_StartThread()
*/
/**
* @brief
* Starts the gps thread.
*
*
********************************************************/
int gpsd_StartThread(void)
{
#ifdef POLLED
	pthread_t thread;
#endif
	int rval;

	printf("gpsd_StartThread()\n");
	
//	rval = pthread_create( &thread, NULL, gpsd_mainloop, (void*) NULL);

	return(rval);
}

